/**

This file is part of MaCI/GIMnet.

MaCI/GIMnet is free software: you can redistribute it and/or modify it 
under the terms of the GNU Lesser General Public License as published 
by the Free Software Foundation, either version 3 of the License, or 
(at your option) any later version.

MaCI/GIMnet is distributed in the hope that it will be useful, but WITHOUT 
ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or 
FITNESS FOR A PARTICULAR PURPOSE.  See the GNU Lesser General Public 
License for more details.

You should have received a copy of the GNU Lesser General Public 
License along with GIMnet. (See COPYING.LESSER) If not, see 
<http://www.gnu.org/licenses/>.

**/

#define MAP_FILE_NAME "../../res/autmap-small.txt"

#include<stdio.h>
#include<stdlib.h>
#include<math.h>
#include <string>
#include "settings.hpp"
#include "TOccupancyGrid.h" ///< Occupancy grid class
#include "CParticleFilter.h"
#include "glutImageGUI.h"   ///< For visualisation
#include "JPEGEncoder.hpp"  ///< For saving JPEG
#include "BetBdtLocalizer.h"
		
float topi(float da){
	if(da>M_PI)while(da>M_PI) da-=2.0*M_PI;
	else if(da<-M_PI)while(da<-M_PI) da+=2.0*M_PI;
	return da;
}

///////////////////////////////////////////////////////////////////////////////////
///////////////////////////////////////////////////////////////////////////////////
/// Read Scan measurement file with arbitrary number of items (and allowing white space before '\n')
///////////////////////////////////////////////////////////////////////////////////
///////////////////////////////////////////////////////////////////////////////////
int forwardString(const char *buf, int begin,int items){
		int i=begin+1;
		int cnt = 0;
		while(cnt<items){
				if(buf[i]==' ') cnt++;
				i++;
		}
		return i;
}

/// For reading the data from file
int readScanLine(FILE *f,Grid::TScan &meas, Grid::pose &p ){
		float time_s;
		int i=0;
		char in_line[16096];
		char *ret;
		char c=0;
		
		ret = fgets (in_line, 16096, f );
		if(ret==NULL){
				if(feof(f)){
						fprintf(stderr,"Reached end of file -- Alles klar!\n");
						return 0;
				}else{
						fprintf(stderr,"Some error occurred...sorry contact Jari :)\n");
						return 1;
				}
		}
		int cnt=0;
		while( (c!='\n')){
				if(in_line[i]==' ') cnt++;
				else if(in_line[i]=='\n') break;
				i++;
		}
		//fprintf(stderr,"There is %d floats in this file (= %d meas points)\n",cnt,(cnt-3)/2);
		sscanf(in_line,"%f %f %f ",&p.x,&p.y,&p.a); ///< Pose header
		int pointer_ind=0;
		pointer_ind = forwardString(in_line,0,3); ///search the next begin for float
		meas.N = (cnt-3)/2;
		i =0;
		while(i<((cnt-3)/2)){
				sscanf((in_line+pointer_ind),"%f %f",&meas.r[i],&meas.a[i]);
				pointer_ind = forwardString(in_line,pointer_ind,2); ///search the next begin for float
				i++;
		}
	
		return 0;
}

//////////////////////////////////////////////////////////////////////////////////
//////////////////////////////////////////////////////////////////////////////////
///DRAWING STUFF
//////////////////////////////////////////////////////////////////////////////////
//////////////////////////////////////////////////////////////////////////////////
void setRGBpix(unsigned char *rgb,int i,int j, int Nx, unsigned char r,unsigned char g, unsigned char b ){
		rgb[3*Nx*j+3*i]=r;
		rgb[3*Nx*j+3*i+1]=g;
		rgb[3*Nx*j+3*i+2]=b;
}

void setRGBmap(unsigned char *rgb,TOccupancyGrid *gr, unsigned char r,unsigned char g, unsigned char b){
		float *data = gr->getDataPtr();
		int Nx,Ny;
		gr->getDim(Nx,Ny);
		float v;
	
		for(int j=0;j<Ny;j++){
				for(int i=0;i<Nx;i++){
						v = data[Nx*j+i];
						//if(v>0.6) v = 0.99 ; else v = 0.1; 
				
						setRGBpix(rgb,i,j, Nx, r*v,g*v,b*v);
				}
		}	
}

void setRGBpixBold(unsigned char *rgb,int i,int j, int Nx, unsigned char r,unsigned char g, unsigned char b){
		for(int ii=i-1;ii<i+1;ii++){
				for(int jj=j-1;jj<j+1;jj++){
						setRGBpix(rgb,ii,jj,Nx,r,g,b);
				}
		}

}

void drawLine(unsigned char *RGB, int Nx,int Ny,float resolution,   ///Grid parameters
							uint pix, uint piy,float angle, float length, ///< Start pixel, angle to draw and linelength
							unsigned char r,unsigned char g,unsigned char b){
									float dy = sin(angle);
									float dx = cos(angle);
									float rr=0;
									int i=0;
									uint x,y;
									while(rr<length){
											x = pix + i*dx;
											y = piy - i*dy;
											setRGBpixBold(RGB,x,y, Nx, r,g,b);
											i++;
											rr+=resolution;
									}
}

void drawLineThin(unsigned char *RGB, int Nx,int Ny,float resolution,   ///Grid parameters
							uint pix, uint piy,float angle, float length, ///< Start pixel, angle to draw and linelength
							unsigned char r,unsigned char g,unsigned char b){
									float dy = sin(angle);
									float dx = cos(angle);
									float rr=0;
									int i=0;
									uint x,y;
									while(rr<length){
											x = pix + i*dx;
											y = piy - i*dy;
											setRGBpix(RGB,x,y, Nx, r,g,b);
											i++;
											rr+=resolution;
									}
							}

void drawCross(unsigned char *RGB, int Nx,int Ny,float resolution,   ///Grid parameters
							uint pix, uint piy, float length, ///< Start pixel, angle to draw and linelength
							unsigned char r,unsigned char g,unsigned char b){
		float angle = M_PI/4.0;
		for(int i=0;i<3;i++){
				drawLine(RGB, Nx,Ny,resolution, pix, piy,angle, length, r,g,b);
				angle+=M_PI/1.9;
		}
}


void saveJPEG(const char *fname, unsigned char *rgb,int Nx, int Ny){
		CJPEGEncoder encoder;
		encoder.SetRGBDataPtr((const char *)rgb,Nx,Ny);
		FILE *f = fopen(fname,"wb");
		fwrite(encoder.GetJPEGDataPtr(), 
					 encoder.GetJPEGDataSize(), 
					 1, f);
		fclose(f);
		
}

float randomi(){
	return (2.0 * (float)(rand()%10000) - 1.0)/10000.0;
}

//////////////////////////////////////////////////////////////////////////////////
//////////////////////////////////////////////////////////////////////////////////
//////////////////////////////////////////////////////////////////////////////////

void saveScan(const char *fname, Grid::TScan &s){
		FILE *f = fopen(fname,"wt");
		if(f==NULL){
				fprintf(stderr,"Not good - unable to save!\n");
				exit(1);
		}

		for(int i=0;i<s.N;i++){
				fprintf(f,"%f %f\n",s.r[i],s.a[i]);
		}
		fclose(f);
}

void saveMeasurement(FILE *f, Grid::TScan &s, Grid::pose &p){
		if(f==NULL) return;
		fprintf(f,"%.4f %.4f %.4f ",p.x,p.y,p.a);
		for(int i=0;i<s.N-1;i++){
				fprintf(f,"%.3f %.3f ",s.r[i],s.a[i]);
		}
		fprintf(f,"%.3f %.3f\n",s.r[s.N-1],s.a[s.N-1]);
		fflush(f);
		
}

void printUsage(){
		fprintf(stderr,"\n-----------------------------------------------------------------\n");
		fprintf(stderr,"Matches a logged scan file to a given map\n");
		fprintf(stderr,"Usage: ./mapping_tool [Method] args... \n");
		fprintf(stderr,"[Method] Number of the method to be used:\n");
		fprintf(stderr,"\t0 [in_data_file]: Set measurement file to Occupancy Grid\n");
		fprintf(stderr,"\t\t[in_data_file]: A full path to file containing data\n");
		fprintf(stderr,"\t1 [in_map] [in_data_file]: Match the whole dataset to Global map\n");
		fprintf(stderr,"\t\t[in_map] full path to map file\n");
		fprintf(stderr,"\t\t[in_data_file]: A full path to file containing data\n");
		fprintf(stderr,"\t2 [in_map] [in_data_file]: Match Local SLAM maps to Global map\n");
		fprintf(stderr,"\t\t[in_map] full path to map file\n");
		fprintf(stderr,"\t\t[in_data_file]: A full path to file containing data\n");
		fprintf(stderr,"\t3: [in_line_map] [in_map] [in_data_file]: Remap using old occupancy grid, a CAD map and some dataset\n");
		fprintf(stderr,"\t\t[in_line_map] full path to CAD map file\n");
		fprintf(stderr,"\t\t[in_map] full path to map file. NOTE MUST BE OCCU MAP!!\n");
		fprintf(stderr,"\t\t[in_data_file]: A full path to file containing data\n");
		fprintf(stderr,"\t4: [in_map] [in_data_file]: Global localization\n");
		fprintf(stderr,"\t\t[in_map] full path to map file. NOTE MUST BE OCCU MAP!!\n");
		fprintf(stderr,"\t\t[in_data_file]: A full path to file containing data\n");
		fprintf(stderr,"\t5: [in_map] [in_data_file]: Matches Local SLAM Maps to Global SLAM map\n");
		fprintf(stderr,"\t\t[in_data_file]: A full path to file containing data\n");
		//fprintf(stderr,"[in_map] full path to map file\n");
		//fprintf(stderr,"[in_map] full path to scan measurement file\n");
		fprintf(stderr,"\nThe output is saved to 'out_result.jpg'\n");
		fprintf(stderr,"-----------------------------------------------------------------\n\n");
}

bool initializeLocalizers(gim::CSettings &settings, CBetBdtLocalizer **glocalizer, 
													 CBetBdtLocalizer **slammer, std::string map, bool isOccuMapFile){
		float grid_size, resolution;
		float paramTh, paramfdist;
		
		settings.GetValue("BB-global.resolution", resolution);
		settings.GetValue("BB-global.distth", paramTh);
		settings.GetValue("BB-global.fdist", paramfdist);
		
		dPrint(1,"Initializing Global matcher:");
		dPrint(1,"map='%s' th=%.2f fdist=%.2f resolution=%.2f",map.c_str(),paramTh,paramfdist,resolution);
		
		if(isOccuMapFile) *glocalizer = new CBetBdtLocalizer(map.c_str());
		else *glocalizer = new CBetBdtLocalizer(map.c_str(), resolution);
		
		(*glocalizer)->tuneParameters(paramfdist,paramTh);
		
		settings.GetValue("BB-local.grid_map_size", grid_size);
		settings.GetValue("BB-local.resolution", resolution);
		settings.GetValue("BB-local.distth", paramTh);
		settings.GetValue("BB-local.fdist", paramfdist);
		
		dPrint(1,"Initializing SLAM matcher:");
		dPrint(1,"Grid_size='%.2f' th=%.2f fdist=%.2f resolution=%.2f",grid_size, paramTh,paramfdist, resolution);
		*slammer = new CBetBdtLocalizer(grid_size,resolution);
		(*slammer)->tuneParameters(paramfdist,paramTh);

		return true;
}
/**
* Sets a whole measurement file to the Occupancy Grid
* @param &settings Settings class
* @param *O Initialized Occupancy Grid 
* @param *meas_file The data log file
* @param *rgb Initialized image data ptr
* @param window_id The id of the window which the grid is drawn (Negative disables drawing)
**/
void setUpOccuMap(gim::CSettings &settings, TOccupancyGrid *O, const char *meas_file, unsigned char *rgb, int window_id){
		FILE *log = fopen(meas_file,"rt"); ///< data file
		if(log == NULL){
				fprintf(stderr,"Unable to open the log file\n");
				exit(1);
		}
		float max_laser_dist;
		settings.GetValue("BB-all.max_laser_distance", max_laser_dist);
		
		int cnt = 0;
		Grid::pose pos;
		std::vector< Grid::pose > poses;    ///All poses for previous map
		
		float enough_scan_poinsts;
		settings.GetValue("BB-all.enough_scan_points", enough_scan_poinsts);
		
		Grid::TScan meas;
		meas.alloc(enough_scan_poinsts);
		int Nx,Ny;
		O->getDim(Nx,Ny);
		while(!feof(log)){
				readScanLine(log,meas,pos);
				poses.push_back(pos);
				meas.filt(max_laser_dist);
				O->setScanFillProb(pos.x, pos.y ,pos.a,meas,0.3,0.9);
				
				if(window_id>=0){
					if(cnt%150==0){
							O->fillRGB(rgb,1.0/255.0);
							for(int i=0;i<poses.size();i++){
									Grid::TPixel pix = O->toPixel(Grid::TCoord2D(poses[i].x,poses[i].y));
									setRGBpix(rgb,pix.x,pix.y, Nx, 255,0, 0 );
							}
							displayImage(window_id, rgb,Ny,Nx);
					}
				}
				cnt++;
		}
		
		fprintf(stderr,"Done! Now saving data and result image\n");
		O->fillRGB(rgb,1.0/255.0); 
		saveJPEG("Occu-Map-result.jpg", rgb,Nx, Ny);
		O->save("occu-map.occu");
		
		fprintf(stderr,"All Done!!\n");
}
/**
* Match the whole data set to map
* Uses localized data to match the whole data set to the map
**/
void globalMatch(gim::CSettings &settings,
								 CBetBdtLocalizer *glocalizer,
								 TOccupancyGrid *O, 
								 unsigned char *grgb, int window_id)
{
		int Nx,Ny;
		glocalizer->grid->getDim(Nx,Ny);
		
		int enableVisualization;
		settings.GetValue("BB-all.enableVisualization", enableVisualization);
		
		CMatchStorage matches(50);
		CEnvironmentMeasurement2D meas(O,0.9); ///Make a measurement
		settings.GetValue("BB-global.initPoseX", meas.frame.x_ref);
		settings.GetValue("BB-global.initPoseY", meas.frame.y_ref);
		settings.GetValue("BB-global.initPoseA", meas.frame.a_ref);
		
		settings.GetValue("BB-global.searchX", meas.frame.x_err);
		settings.GetValue("BB-global.searchY", meas.frame.y_err);
		settings.GetValue("BB-global.searchA", meas.frame.a_err);
		
		dPrint(1,"Num_meas='%d' InitP (%.2f,%.2f,%.2f) Search: (%.2f,%.2f,%.2f)",meas.size(),
					 meas.frame.x_ref,meas.frame.y_ref,meas.frame.a_ref,
					 meas.frame.x_err,meas.frame.y_err,meas.frame.a_err);
		
		
		setRGBmap(grgb,glocalizer->grid, 0,0,255);
		float px,py,dl,da;
		
		///Plot the measurement to Initial Guess
		for(unsigned int i=0; i<meas.points.size();i++){
				dl = meas.points[i].getDist();
				da = meas.points[i].getAngle();
								
				px = dl * cos(da+meas.frame.a_ref)+meas.frame.x_ref;
				py = dl * sin(da+meas.frame.a_ref)+meas.frame.y_ref;
				Grid::TPixel pix=glocalizer->grid->toPixel(Grid::TCoord2D(px,py));				
				setRGBpixBold(grgb,pix.x,pix.y, Nx, 0,255, 0);
		}
		fprintf(stderr,"Initial Guess...\n");
		if(enableVisualization) displayImage(window_id, grgb,Ny,Nx);
		fprintf(stderr,"Matching....");
			
		glocalizer->localize(meas,matches);
		fprintf(stderr," Done \n");
		matches.sort();
		fprintf(stderr,"\n###################################################\n");
		fprintf(stderr,"Pose estimate = (%f , %f , %f)\n",matches.storage[0].x,matches.storage[0].y,matches.storage[0].a);
		fprintf(stderr,"###################################################\n\n");
		setRGBmap(grgb,glocalizer->grid, 0,0,255);
		///Plot the measurement to best matching pose
		//float px,py,dl,da;
		for(unsigned int i=0; i<meas.points.size();i++){
				dl = meas.points[i].getDist();
				da = meas.points[i].getAngle();
								
				px = dl * cos(da+matches.storage[0].a) + matches.storage[0].x;
				py = dl * sin(da+matches.storage[0].a) + matches.storage[0].y;
				Grid::TPixel pix=glocalizer->grid->toPixel(Grid::TCoord2D(px,py));				
				setRGBpixBold(grgb,pix.x,pix.y, Nx, 255,0, 0);
		}
		Grid::TPixel pix=glocalizer->grid->toPixel(Grid::TCoord2D(matches.storage[0].x,matches.storage[0].y));	
		drawCross(grgb, Nx,Ny,glocalizer->grid->getResolution(),   ///Grid parameters
							pix.x, pix.y, 1.0, ///< Start pixel, angle to draw and linelength
							0,255,0);
		if(enableVisualization) displayImage(window_id, grgb,Ny,Nx);
}
/////////////////////////////////////////////////////////////////////////////
/////////////////////////////////////////////////////////////////////////////
/// SLAM-MAP Matcher
/// Matches local SLAM maps to the global map
///
/////////////////////////////////////////////////////////////////////////////
/////////////////////////////////////////////////////////////////////////////
void globalSLAMMatch(gim::CSettings &settings,
										const char *meas_file, 
										CBetBdtLocalizer *glocalizer,
										CBetBdtLocalizer *slammer, 
										unsigned char *grgb,unsigned char *srgb)
{
		CMatchStorage matches(50);
		Grid::pose pos;
		std::vector< Grid::pose > poses;    ///All poses for previous map
		FILE *logfilu = fopen("out_scan_meas.txt","wt");
		int enableVisualization;
		settings.GetValue("BB-all.enableVisualization", enableVisualization);
		
		FILE *log = fopen(meas_file,"rt"); ///< data file
		if(log == NULL){
				fprintf(stderr,"Unable to open the log file\n");
				exit(1);
		}
		float max_laser_dist;
		settings.GetValue("BB-all.max_laser_distance", max_laser_dist);
		fprintf(stderr,"Max allowed Laser distance='%.3f'",max_laser_dist);
		float enough_scan_poinsts;
		settings.GetValue("BB-all.enough_scan_points", enough_scan_poinsts);
		
		Grid::TScan meas;
		meas.alloc(enough_scan_poinsts);
		std::vector< std::vector<float> > scans; ///All scans for previous map
		std::vector<float> tmp_scan;
		
		Grid::pose init_est,diff;
		Grid::pose last,current;
		Grid::pose reference_pose, pose_now, global_ref_pose, global_search;
		
		///Read the initial estimate from the file (FIXME: Use global estimate?)
		settings.GetValue("BB-global.initPoseX", global_ref_pose.x);
		settings.GetValue("BB-global.initPoseY", global_ref_pose.y);
		settings.GetValue("BB-global.initPoseA", global_ref_pose.a);
		settings.GetValue("BB-global.searchX", global_search.x);
		settings.GetValue("BB-global.searchY", global_search.y);
		settings.GetValue("BB-global.searchA", global_search.a);
		CEnvironmentMeasurement2D global_meas;
		global_meas.frame.setError(global_search.x,global_search.y,global_search.a);		
		
		CEnvironmentMeasurement2D slam_meas;	
		settings.GetValue("BB-local.searchX", slam_meas.frame.x_err);
		settings.GetValue("BB-local.searchY", slam_meas.frame.y_err);
		settings.GetValue("BB-local.searchA", slam_meas.frame.a_err);
		
		float edge_th;
		settings.GetValue("BB-local.toEdge", edge_th);
		
		float grid_size;
		settings.GetValue("BB-local.grid_map_size", grid_size);
		
		int Nx,Ny;
		glocalizer->grid->getDim(Nx,Ny);
		int sNx,sNy;
		slammer->grid->getDim(sNx,sNy);
		
		///Set the first scan
		readScanLine(log,meas,current);
		meas.filt(max_laser_dist);
		slammer->setMeasurementToMap(meas, 0, 0,  0, 0.99);
		glocalizer->setMeasurementToMap(meas, 0, 0,  0, 0.99);
		tmp_scan.clear();
		for(int i=0;i<meas.N;i++){
				tmp_scan.push_back(meas.r[i]);
		}
		scans.push_back(tmp_scan);
		poses.clear();
		poses.push_back(init_est);
		
		
		current = last;
		int counter=0;
		
		while(!feof(log)){
				readScanLine(log,meas,current);
				
				meas.filt(max_laser_dist);
				///Save scans and poses 
				tmp_scan.clear();
				for(int i=0;i<meas.N;i++){
						tmp_scan.push_back(meas.r[i]);
				}
				scans.push_back(tmp_scan);
				
				diff.setToDifferentialPose(current,last);
				init_est = init_est.integrateDifferential(diff);
				last=current;
				
				///Put scan to env meas class
				TMeasurementPoint p;
				slam_meas.points.clear();
				//fprintf(stderr,"N=%d\n",meas.N);
				for(int i=0;i<meas.N;i++){
						p.x = meas.r[i]*cos(meas.a[i]);
						p.y = meas.r[i]*sin(meas.a[i]);
						slam_meas.addPoint(p);
				}
				slam_meas.frame.setFrame(init_est.x,init_est.y,init_est.a);
				if(slammer->localize(slam_meas,matches)){
						matches.sort();
						init_est.set(matches.storage[0].x,matches.storage[0].y,matches.storage[0].a);
						pose_now = reference_pose.integrateDifferential(init_est); ///< Current pose
						poses.push_back(init_est); ///Save pose for this scan 
						slammer->setMeasurementToMap(meas,init_est.x,init_est.y,init_est.a,0.75);
						saveMeasurement(logfilu,meas,pose_now);
				}
				////////////////////////////////////////////////////////////////////////////
				/// Resetting the map
				////////////////////////////////////////////////////////////////////////////
				pos = init_est;
				float toPos = sqrt(pos.x*pos.x+pos.y*pos.y);
				float toEdge;
				toEdge = (grid_size/2.0)-toPos;			
				if(toEdge < edge_th || feof(log)){
						global_meas.setOccupancyGrid(slammer->grid, 0.8); ///< set grid to 2D measurement 
						global_meas.frame.setFrame(global_ref_pose.x,global_ref_pose.y,global_ref_pose.a);	
						
						/////////////////////////////////////////////////////////////////////////////
						///////////////////// SAVE INITIAL ESTIMATE /////////////////////////////////
						setRGBmap(grgb,glocalizer->grid, 0,0,255);
						Grid::TPixel pixx;
						
						for(int i=0;i<global_meas.size();i++){
								float r =sqrt(
												global_meas.points[i].x*global_meas.points[i].x+
												global_meas.points[i].y*global_meas.points[i].y);
								float a = atan2(global_meas.points[i].y,global_meas.points[i].x);
								pixx = glocalizer->grid->toPixel(
												Grid::TCoord2D(global_ref_pose.x+r*cos(a+global_ref_pose.a),
																			 global_ref_pose.y+r*sin(a+global_ref_pose.a)));
								setRGBpixBold(grgb,pixx.x,pixx.y, Nx, 255,0, 0);
						}
						char buf[50];
						sprintf(buf,"%05d_Initial-map.jpg",counter);
						saveJPEG(buf, grgb,Nx, Ny);
						/////////////////////////////////////////////////////////////////////////////
						/////////////////////////////////////////////////////////////////////////////
						
						glocalizer->localize(global_meas,matches);
						matches.sort();
						fprintf(stderr,"\n###################################################\n");
						fprintf(stderr,"Pose estimate = (%f , %f , %f)\n",matches.storage[0].x,matches.storage[0].y,matches.storage[0].a);
						fprintf(stderr,"###################################################\n\n");
						 ///< this is the matched reference
						global_ref_pose.set(matches.storage[0].x,matches.storage[0].y,matches.storage[0].a);
						
						
						
						
						///FIXME: the first scan is missing
						///Add measurements to the map
						for(unsigned int i=0;i<scans.size()-1;i++){
								for(int j=0;j<meas.N;j++){
										meas.r[j]=scans[i+1][j];
								}
								diff.setToDifferentialPose(poses[i+1],poses[i]);
								global_ref_pose = global_ref_pose.integrateDifferential(diff);
								glocalizer->setMeasurementToMap(meas,global_ref_pose.x,global_ref_pose.y,global_ref_pose.a ,0.8,false);	
						}
						global_ref_pose.set(matches.storage[0].x,matches.storage[0].y,matches.storage[0].a);
						reference_pose = global_ref_pose;
						global_ref_pose = global_ref_pose.integrateDifferential(init_est); ///< Current pose
						
						slammer->grid->setGridToValue(0.5); ///< Clear map
						
						reference_pose = reference_pose.integrateDifferential(init_est); ///< Current pose
						
						
						for(unsigned int i=1;i<scans.size()-2;i+=1){
								for(int j=0;j<meas.N;j++){
										meas.r[j]=scans[scans.size()-i-1][j]; ///< starting from the last measurement
										if(meas.r[j]>max_laser_dist) meas.r[j]=0; 
								}
								diff.setToDifferentialPose(poses[scans.size()-1-i],poses[scans.size()-1]);
								slammer->setMeasurementToMap(meas,diff.x,diff.y,diff.a,0.8,false);
						}
						slammer->updateDistTransform();
						
							
						pos.set(0,0,0); ///< initialize pose
						init_est.set(0,0,0);
						
						glocalizer->updateDistTransform();
						scans.clear();
						poses.clear();
						
						tmp_scan.clear();
						for(int i=0;i<meas.N;i++){
								tmp_scan.push_back(meas.r[i]);
						}
						scans.push_back(tmp_scan);
						poses.clear();
						poses.push_back(init_est);
						
						//////////////////////////////////////////////////////////////////////////////////
						///VISU
						//////////////////////////////////////////////////////////////////////////////////
						if(enableVisualization){
							setRGBmap(grgb,glocalizer->grid, 0,0,255);
		
							Grid::TPixel pix=glocalizer->grid->toPixel(Grid::TCoord2D(matches.storage[0].x,matches.storage[0].y));	
							drawCross(grgb, Nx,Ny,glocalizer->grid->getResolution(),   ///Grid parameters
											pix.x, pix.y, 1.0, ///< Start pixel, angle to draw and linelength
											0,255,0);
							displayImage(0, grgb,Ny,Nx);
						}
						////////////////////////////////////////////////////////////////////////////////
						
						setRGBmap(grgb,glocalizer->grid, 0,0,255);
						sprintf(buf,"%05d_global-map.jpg",counter);
						saveJPEG(buf, grgb,Nx, Ny);
				}
				counter++;
				if(enableVisualization){
					if(counter%10==0){
							setRGBmap(srgb,slammer->grid, 0,0,255);
							displayImage(1, srgb,sNy,sNx);
					}
				}
				////////////////////////////////////////////////////////////////////////////////
				
		}///While()
		
		fclose(logfilu);
}

/////////////////////////////////////////////////////////////////////////////
/////////////////////////////////////////////////////////////////////////////
/// Localization tester
/// Matches to Occupancy grid using odometry and range data 
/// Collects the measurements to the occupancy grid and then matches to the 
/// map
/////////////////////////////////////////////////////////////////////////////
/////////////////////////////////////////////////////////////////////////////
void globalLocalization(gim::CSettings &settings,
												const char *meas_file, 
												CBetBdtLocalizer *glocalizer,
												TOccupancyGrid *O, 
												unsigned char *grgb,unsigned char *srgb)
{
		CMatchStorage matches(50);
		Grid::pose pos;
		std::vector< Grid::pose > poses;    ///All poses for previous map
		std::vector< Grid::pose > trajectory;    ///All poses for previous map
		std::vector< Grid::pose > corrected_traj;    ///All poses for previous map
		int enableVisualization;
		settings.GetValue("BB-all.enableVisualization", enableVisualization);
		
		FILE *log = fopen(meas_file,"rt"); ///< data file
		if(log == NULL){
				fprintf(stderr,"Unable to open the log file\n");
				exit(1);
		}
		float max_laser_dist;
		settings.GetValue("BB-all.max_laser_distance", max_laser_dist);
		
		float enough_scan_poinsts;
		settings.GetValue("BB-all.enough_scan_points", enough_scan_poinsts);
		
		Grid::TScan meas;
		meas.alloc(enough_scan_poinsts);
		std::vector< std::vector<float> > scans; ///All scans for previous map
		std::vector<float> tmp_scan;
		
		Grid::pose init_est,diff;
		Grid::pose last,current;
		Grid::pose reference_pose, pose_now, global_ref_pose, global_search;
		
		///Read the initial estimate from the file (FIXME: Use global estimate?)
		settings.GetValue("BB-global.initPoseX", global_ref_pose.x);
		settings.GetValue("BB-global.initPoseY", global_ref_pose.y);
		settings.GetValue("BB-global.initPoseA", global_ref_pose.a);
		settings.GetValue("BB-global.searchX", global_search.x);
		settings.GetValue("BB-global.searchY", global_search.y);
		settings.GetValue("BB-global.searchA", global_search.a);
		CEnvironmentMeasurement2D global_meas;
		global_meas.frame.setError(global_search.x,global_search.y,global_search.a);		
		
		global_meas.frame.setFrame(global_ref_pose.x,global_ref_pose.y,global_ref_pose.a);
		
		float edge_th;
		settings.GetValue("BB-local.toEdge", edge_th); ///NOTE uses variable from the Local map
		
		float grid_size;
		settings.GetValue("BB-local.grid_map_size", grid_size);
		
		int Nx,Ny;
		glocalizer->grid->getDim(Nx,Ny);
		int sNx,sNy;
		O->getDim(sNx,sNy);
		
		///Set the first scan
		readScanLine(log,meas,current);
		O->setScanFillProb(0, 0 , 0, meas,0.2,0.85);
		
		tmp_scan.clear();
		for(int i=0;i<meas.N;i++){
				tmp_scan.push_back(meas.r[i]);
		}
		scans.push_back(tmp_scan);
		poses.clear();
		poses.push_back(init_est);
		
		
		current = last;
		int counter=0;
		init_est.set(0,0,0);
		while(!feof(log)){
				readScanLine(log,meas,current);
				meas.filt(max_laser_dist);
				///Save scans and poses 
				tmp_scan.clear();
				for(int i=0;i<meas.N;i++){
						tmp_scan.push_back(meas.r[i]);
				}
				scans.push_back(tmp_scan);
				
				diff.setToDifferentialPose(current,last);
				init_est = init_est.integrateDifferential(diff);
				last=current;
				poses.push_back(init_est);
				
				reference_pose = global_ref_pose.integrateDifferential(init_est);
				trajectory.push_back(reference_pose);
				
				O->setScanFillProb(init_est.x, init_est.y , init_est.a, meas,0.2,0.85);
				
				////////////////////////////////////////////////////////////////////////////
				/// Resetting the map
				////////////////////////////////////////////////////////////////////////////
				pos = init_est;
				float toPos = sqrt(pos.x*pos.x+pos.y*pos.y);
				float toEdge;
				toEdge = (grid_size/2.0)-toPos;			
				
				if(toEdge < edge_th || feof(log)){ ///< time to localize
						O->setGridToValue(0.5); ///< initialize the grid
						
						///"Rewind" the data to the Occupancy Grid
						for(unsigned int i=0;i<scans.size()-1;i+=1){
								for(int j=0;j<meas.N;j++){
										meas.r[j]=scans[scans.size()-i-1][j]; ///< starting from the last measurement
										if(meas.r[j]>max_laser_dist) meas.r[j]=0; 
								}
								diff.setToDifferentialPose(poses[scans.size()-1-i],poses[scans.size()-1]);
								O->setScanFillProb(diff.x,diff.y,diff.a, meas,0.2,0.85);
						}
						if(enableVisualization){
										setRGBmap(srgb,O, 0,0,255);
										displayImage(1, srgb,sNy,sNx);
								
						}
						
						/// The pose of the most recent scan
						global_ref_pose=global_ref_pose.integrateDifferential(init_est);
						
						global_meas.setOccupancyGrid(O, 0.8); ///< set grid to 2D measurement 
						global_meas.frame.setFrame(global_ref_pose.x,global_ref_pose.y,global_ref_pose.a);	
						
						glocalizer->localize(global_meas,matches);
						matches.sort();
						fprintf(stderr,"\n###################################################\n");
						fprintf(stderr,"Pose estimate = (%f , %f , %f)\n",
																				matches.storage[0].x,matches.storage[0].y,matches.storage[0].a);
						fprintf(stderr,"###################################################\n\n");
						 ///< this is the matched reference
						global_ref_pose.set(matches.storage[0].x,matches.storage[0].y,matches.storage[0].a);
						init_est.set(0,0,0);
						O->setGridToValue(0.5);
						corrected_traj.push_back(global_ref_pose);
						scans.clear();
						poses.clear();
						tmp_scan.clear();
						
						//////////////////////////////////////////////////////////////////////////////////
						///VISU
						//////////////////////////////////////////////////////////////////////////////////
						setRGBmap(grgb,glocalizer->grid, 0,0,255);
						
						for(int i=0;i<corrected_traj.size();i++){
								Grid::TPixel pix=glocalizer->grid->toPixel(Grid::TCoord2D(corrected_traj[i].x,corrected_traj[i].y));	
								drawCross(grgb, Nx,Ny,glocalizer->grid->getResolution(),   ///Grid parameters
												pix.x, pix.y, 2.0, ///< Start pixel, angle to draw and linelength
												0,255,0);
						}
						if(enableVisualization){
								displayImage(0, grgb,Ny,Nx);
						}
						////////////////////////////////////////////////////////////////////////////////
						char buf[50];
						
						
						for(int i=0;i<trajectory.size();i++){
								Grid::TPixel pix=glocalizer->grid->toPixel(Grid::TCoord2D(trajectory[i].x,trajectory[i].y));
								setRGBpixBold(grgb,pix.x,pix.y, Nx, 255,0, 0);
						}

						sprintf(buf,"%05d_global-map.jpg",counter);
						saveJPEG(buf, grgb,Nx, Ny);
				}
				counter++;
				if(enableVisualization){
						if(counter%10==0){
								setRGBmap(srgb,O, 0,0,255);
								displayImage(1, srgb,sNy,sNx);
						}
				}
				////////////////////////////////////////////////////////////////////////////////
				
		}///While()
}

int main(int argc, char *argv[]){
		const char *measurement_file_name="../../../data/j2b2_measurement.txt";
		std::string configurationFile="bb-mapping.xml";
		std::string mapfilename;
		int enough_scan_poinsts = 400; ///< enough to reserve memory for the scan points in the meas file.
		
		///Localizers
		CBetBdtLocalizer *glocalizer; 
		CBetBdtLocalizer *slammer;
		
		if(argc < 2){ 
				printUsage();
				return 0;
		} 
		int method = atoi(argv[1]);
		
		// Init Debug lib
		debugInit();
		debugSetGlobalDebugLvl(1);
	
		///Open & read settings file
		gim::CSettings settings;
		if(!settings.ReadSettingsXMLFile(configurationFile)){
				dPrint(1,"Failed to read the configuration file!!!");
				return 0;
		}
		int enableVisualization;
		settings.GetValue("BB-all.enableVisualization", enableVisualization);
		
////////////////////////////////////////////////////////////////////////////////////////////		
////////////////////////////////////////////////////////////////////////////////////////////
////////////////////////////////////////////////////////////////////////////////////////////
		switch(method){
			case 0:{ ///< Set data to Occu 
				if(argc != 3){
					printUsage();
					return 0;
				}
				measurement_file_name = argv[2];				
				fprintf(stderr,"\nUsing Method: Setting Data set to Occupancy Grid\n");
				
				///Initialize Occupancy grid 
				float grid_size;
				float occu_reso;
				settings.GetValue("Occu.grid_map_size", grid_size);
				settings.GetValue("Occu.resolution", occu_reso);
				TOccupancyGrid O(grid_size,grid_size,occu_reso,Grid::TCoord2D(0,0));
				O.setGridToValue(0.5);
				int oNx,oNy; /// 'o'	indicates Occupancy Grid
				unsigned char *orgb;
				O.getDim(oNx,oNy);
				orgb = (unsigned char *) malloc(3*oNx*oNy);		
				
				if(enableVisualization){
					addImageWindow(oNx, oNy); ///Window 0 is the global matcher grid
					glutImageGUI_Start();
					setUpOccuMap(settings, &O, measurement_file_name, orgb,0);
				}else{
					setUpOccuMap(settings, &O, measurement_file_name, orgb,-1);
				}
				return 0;
				break;
			}
				////////////////////////////////////////////////////////////////////////////////////////////
				////////////////////////////////////////////////////////////////////////////////////////////
				////////////////////////////////////////////////////////////////////////////////////////////
			case 1:{
				if(argc != 4){
					printUsage();
					return 0;
				}
				mapfilename = argv[2];
				measurement_file_name = argv[3];
		
				fprintf(stderr,"\nUsing Method: Localizing the whole data set to a global map\n");
				
				///Initialize Occupancy grid 
				float grid_size;
				float occu_reso;
				settings.GetValue("Occu.grid_map_size", grid_size);
				settings.GetValue("Occu.resolution", occu_reso);
				TOccupancyGrid O(grid_size,grid_size,occu_reso,Grid::TCoord2D(0,0));
				O.setGridToValue(0.5);
				int oNx,oNy; /// 'o'	indicates Occupancy Grid
				unsigned char *orgb;
				O.getDim(oNx,oNy);
				orgb = (unsigned char *) malloc(3*oNx*oNy);		
				
				/// Initialize localizers
				initializeLocalizers(settings, &glocalizer,&slammer,mapfilename,false);
				
				int gNx,gNy; /// 'g'  indicates Global map
				unsigned char *grgb;
				glocalizer->grid->getDim(gNx,gNy);
				grgb = (unsigned char *) malloc(3*gNx*gNy);	
				
				if(enableVisualization) addImageWindow(gNx,gNy);
				
				if(enableVisualization){
					addImageWindow(oNx, oNy); ///Window 0 is the global matcher grid
					glutImageGUI_Start();
					setUpOccuMap(settings, &O, measurement_file_name, orgb,1);
				}else{
					setUpOccuMap(settings, &O, measurement_file_name, orgb,-1);
				}
				
				globalMatch(settings,glocalizer,&O, grgb,0);
				if(enableVisualization){
					fprintf(stderr,"PRESS CTRL-C TO END\n");
					while(1){
						displayImage(0,grgb,gNx,gNy);
						displayImage(1,orgb,oNx,oNy);
						usleep(1000*100);
					}
				}
				
				return 0;
				break;
			}
			////////////////////////////////////////////////////////////////////////////////////////////
			////////////////////////////////////////////////////////////////////////////////////////////
			////////////////////////////////////////////////////////////////////////////////////////////
			case 2:{
				if(argc != 4){
					printUsage();
					return 0;
				}
				mapfilename = argv[2];
				measurement_file_name = argv[3];
				fprintf(stderr,"\nUsing Method: Setting local SLAM maps to global map\n");
				//////////////////////////////////////////////////////////////////////////////////
				/// Initialize localizers
				initializeLocalizers(settings, &glocalizer,&slammer,mapfilename, false);
				//////////////////////////////////////////////////////////////////////////////////
				int gNx,gNy; /// 'g'  indicates Global map
				int sNx,sNy; /// 's'  indicates SLAM map
				unsigned char *grgb,*srgb;
				
				glocalizer->grid->getDim(gNx,gNy);
				grgb = (unsigned char *) malloc(3*gNx*gNy);
				slammer->grid->getDim(sNx,sNy);
				srgb = (unsigned char *) malloc(3*gNx*gNy);
				if(enableVisualization){
					addImageWindow(gNx, gNy); ///Window 0 is the global matcher grid
					addImageWindow(sNx, sNy); ///Window 1 is the Occupancy Grid Window
					glutImageGUI_Start();
				}
				globalSLAMMatch(settings,measurement_file_name,glocalizer,slammer,grgb,srgb);
				/////////////////////////////////////////////////////////////////////////////////
				glocalizer->save("result-map.occu");
				
				break;
			}
			////////////////////////////////////////////////////////////////////////////////////////////
			////////////////////////////////////////////////////////////////////////////////////////////
			////////////////////////////////////////////////////////////////////////////////////////////
			case 3:{
				if(argc != 5){
					printUsage();
					return 0;
				}
				mapfilename = argv[2];
				char *occumap = argv[3];
				measurement_file_name = argv[4];
				fprintf(stderr,"\nUsing Method: Setting local SLAM maps to global map\n");
				//////////////////////////////////////////////////////////////////////////////////
				/// Initialize localizers
				initializeLocalizers(settings, &glocalizer,&slammer,occumap,true);
				//////////////////////////////////////////////////////////////////////////////////
				float *ptr = glocalizer->grid->getDataPtr();
				for(int i=0;i<glocalizer->grid->size();i++){
					if(ptr[i]>0.85) ptr[i]=0.85;
					else if(ptr[i]<0.2) ptr[i]=0.2;
				}
				/// Set Linemap 
				glocalizer->grid->loadLineMap(mapfilename.c_str(), 1.0,1.0,false,false);
				//////////////////////////////////////////////////////////////////////////////////
				int gNx,gNy; /// 'g'  indicates Global map
				int sNx,sNy; /// 's'  indicates SLAM map
				unsigned char *grgb,*srgb;
				
				glocalizer->grid->getDim(gNx,gNy);
				grgb = (unsigned char *) malloc(3*gNx*gNy);
				slammer->grid->getDim(sNx,sNy);
				srgb = (unsigned char *) malloc(3*sNx*sNy);
				if(enableVisualization){
					addImageWindow(gNx, gNy); ///Window 0 is the global matcher grid
					addImageWindow(sNx, sNy); ///Window 1 is the Occupancy Grid Window
					glutImageGUI_Start();
				}
				globalSLAMMatch(settings,measurement_file_name,glocalizer,slammer,grgb,srgb);
				/////////////////////////////////////////////////////////////////////////////////
				glocalizer->save("result-map.occu");
				break;
			}
			////////////////////////////////////////////////////////////////////////////////////////////
			////////////////////////////////////////////////////////////////////////////////////////////
			////////////////////////////////////////////////////////////////////////////////////////////
				case 4:{
						if(argc != 4){
								printUsage();
								return 0;
						}
						char *occumap = argv[2];
						measurement_file_name = argv[3];
						fprintf(stderr,"\nUsing Method: Localizing with Odometry and Occumap\n");
						//////////////////////////////////////////////////////////////////////////////////
						/// Initialize localizers
						initializeLocalizers(settings, &glocalizer,&slammer,occumap,true);
						//////////////////////////////////////////////////////////////////////////////////
						int gNx,gNy; /// 'g'  indicates Global map
						int sNx,sNy; /// 's'  indicates SLAM map
						unsigned char *grgb,*srgb;
				
						glocalizer->grid->getDim(gNx,gNy);
						grgb = (unsigned char *) malloc(3*gNx*gNy);
						slammer->grid->getDim(sNx,sNy);
						srgb = (unsigned char *) malloc(3*sNx*sNy);
						
						if(enableVisualization){
								addImageWindow(gNx, gNy); ///Window 0 is the global matcher grid
								addImageWindow(sNx, sNy); ///Window 1 is the Occupancy Grid Window
								glutImageGUI_Start();
						}
						
						globalLocalization(settings,
															 measurement_file_name, 
															 glocalizer,
															 slammer->grid, 
															 grgb,srgb);
						
						return 0;
						break;
				}
			////////////////////////////////////////////////////////////////////////////////////////////
			////////////////////////////////////////////////////////////////////////////////////////////
			////////////////////////////////////////////////////////////////////////////////////////////
				case 5:{
						if(argc != 3){
								printUsage();
								return 0;
						}

						measurement_file_name = argv[2];
						fprintf(stderr,"\nUsing Method: Setting local SLAM maps to global SLAM map\n");
						//////////////////////////////////////////////////////////////////////////////////
						/// Initialize localizers
						float grid_size, resolution,size;
						float paramTh, paramfdist;
		
						settings.GetValue("BB-global.resolution", resolution);
						settings.GetValue("BB-global.distth", paramTh);
						settings.GetValue("BB-global.fdist", paramfdist);
						settings.GetValue("Occu.grid_map_size",size);
						dPrint(1,"Initializing Empty Global matcher:");
						dPrint(1,"th=%.2f fdist=%.2f resolution=%.2f size=%.2f",paramTh,paramfdist,resolution,size);
		
						glocalizer = new CBetBdtLocalizer(size,resolution);
						glocalizer->tuneParameters(paramfdist,paramTh);
		
						settings.GetValue("BB-local.grid_map_size", grid_size);
						settings.GetValue("BB-local.resolution", resolution);
						settings.GetValue("BB-local.distth", paramTh);
						settings.GetValue("BB-local.fdist", paramfdist);
		
						dPrint(1,"Initializing SLAM matcher:");
						dPrint(1,"Grid_size='%.2f' th=%.2f fdist=%.2f resolution=%.2f",grid_size, paramTh,paramfdist, resolution);
						slammer = new CBetBdtLocalizer(grid_size,resolution);
						slammer->tuneParameters(paramfdist,paramTh);
						//////////////////////////////////////////////////////////////////////////////////
						int gNx,gNy; /// 'g'  indicates Global map
						int sNx,sNy; /// 's'  indicates SLAM map
						unsigned char *grgb,*srgb;
				
						glocalizer->grid->getDim(gNx,gNy);
						grgb = (unsigned char *) malloc(3*gNx*gNy);
						slammer->grid->getDim(sNx,sNy);
						srgb = (unsigned char *) malloc(3*sNx*sNy);
						if(enableVisualization){
								addImageWindow(gNx, gNy); ///Window 0 is the global matcher grid
								addImageWindow(sNx, sNy); ///Window 1 is the Occupancy Grid Window
								glutImageGUI_Start();
						}
						globalSLAMMatch(settings,measurement_file_name,glocalizer,slammer,grgb,srgb);
						/////////////////////////////////////////////////////////////////////////////////
						glocalizer->save("result-map.occu");
						break;
				}
				////////////////////////////////////////////////////////////////////////////////////////////
				////////////////////////////////////////////////////////////////////////////////////////////
				////////////////////////////////////////////////////////////////////////////////////////////
			default:
				fprintf(stderr,"Invalid method!!\n");
				return 0;
				break;
		}
		
		return 0;
}		